#!/usr/bin/env bash
set -euo pipefail

# Resolve workspace root from this script location
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
WORKSPACE_DIR="$(cd "${SCRIPT_DIR}/../../.." && pwd)"
REPO_ROOT_DIR="$(cd "${WORKSPACE_DIR}/.." && pwd)"

# Source ROS workspace if available
if [[ -f "${WORKSPACE_DIR}/devel/setup.bash" ]]; then
	# shellcheck source=/dev/null
	source "${WORKSPACE_DIR}/devel/setup.bash"
fi

RUN_RVIZ=false
RVIZ_CONFIG_PATH=""

# Parse arguments
while [[ $# -gt 0 ]]; do
	case "$1" in
		--rviz)
			RUN_RVIZ=true
			shift || true
			if [[ $# -gt 0 && "$1" != --* ]]; then
				RVIZ_CONFIG_PATH="$1"
				shift || true
			fi
			;;
		--rviz=*)
			RUN_RVIZ=true
			RVIZ_CONFIG_PATH="${1#*=}"
			shift || true
			;;
		*)
			echo "未知参数: $1"
			echo "用法: $0 [--rviz [rviz_config_path]]"
			exit 1
			;;
	esac
done

if ${RUN_RVIZ} && [[ -z "${RVIZ_CONFIG_PATH}" ]]; then
	RVIZ_CONFIG_PATH="${REPO_ROOT_DIR}/rviz/setting.rviz"
fi

if ${RUN_RVIZ} && [[ -n "${RVIZ_CONFIG_PATH}" ]] && [[ ! -f "${RVIZ_CONFIG_PATH}" ]]; then
	echo "警告: RViz 配置文件未找到: ${RVIZ_CONFIG_PATH}"
fi

echo "[导航] 启动 RealSense 服务: realsense"
sudo systemctl start realsense

echo "[导航] 等待 RealSense 服务运行中..."
for i in {1..20}; do
	if systemctl is-active --quiet realsense; then
		echo "[导航] RealSense 服务已运行"
		break
	fi
	sleep 0.5
done

echo "[导航] 启动 ROS: roslaunch allmovebase all.launch"

if ${RUN_RVIZ}; then
	roslaunch --wait allmovebase all.launch &
	ROSLAUNCH_PID=$!

	if [[ -n "${RVIZ_CONFIG_PATH}" && -f "${RVIZ_CONFIG_PATH}" ]]; then
		echo "[导航] 启动 RViz: ${RVIZ_CONFIG_PATH}"
		rviz -d "${RVIZ_CONFIG_PATH}" &
		RVIZ_PID=$!
	else
		echo "[导航] 启动 RViz (无配置文件)"
		rviz &
		RVIZ_PID=$!
	fi

	trap 'kill ${ROSLAUNCH_PID} ${RVIZ_PID:-} 2>/dev/null || true' SIGINT SIGTERM
	wait ${ROSLAUNCH_PID}
else
	exec roslaunch --wait allmovebase all.launch
fi


